/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_MagTrq.c
 *
 *  Created on: 2022
 *      Author: 64435
 */


#include "LOS_MagTrq.h"

//磁力矩器方向正负定义0：不翻转原始定义 1：翻转原始定义 （原始定义：磁力矩器驱动接口+高于-为正，-高于+为负）
uint8_t MTPOL_X1;
uint8_t MTPOL_Y1;
uint8_t MTPOL_Z1;
uint8_t MTPOL_X2;
uint8_t MTPOL_Y2;
uint8_t MTPOL_Z2;

void LOS_MagTrq_Set(uint32_t pin, uint32_t dir, uint32_t duty)
{
    uint32_t PWMNUM = 0;
    uint32_t pwmpin = 10; //因为pin10啥也没连，比较安全
    switch(pin)
    {
        case DIRX1_PIN:
        PWMNUM = LOS_PWMX1;
        break;

        case DIRY1_PIN:
        PWMNUM = LOS_PWMY1;
        break;

        case DIRZ1_PIN:
        PWMNUM = LOS_PWMZ1;
        break;

        case DIRX2_PIN:
        PWMNUM = LOS_PWMX2;
        break;

        case DIRY2_PIN:
        PWMNUM = LOS_PWMY2;
        break;

        case DIRZ2_PIN:
        PWMNUM = LOS_PWMZ2;
        break;

        default:
        duty = 0;
    }

    gioSetBit(hetPORT1, pin, dir);
    LOS_PWM_SetSignal(PWMNUM, duty, 250000);
}

void LOS_MagTrq_TC(uint8_t nID, int8_t du)
{
    uint32_t dir = 0;
    uint32_t duty = 0;
    uint32_t pin = 0;

    switch(nID)
    {
        case nID_MAGTRQX1:
        pin = DIRX1_PIN;
        if (du < 0)
        {
            duty = -du;
            dir = 0 + MTPOL_X1;
        }
        else if (du >= 0)
        {
            duty = du;
            dir = 1 - MTPOL_X1;
        }
        break;

        case nID_MAGTRQY1:
        pin = DIRY1_PIN;
        if (du < 0)
        {
            duty = -du;
            dir = 0 + MTPOL_Y1;
        }
        else if (du >= 0)
        {
            duty = du;
            dir = 1 - MTPOL_Y1;
        }
        break;

        case nID_MAGTRQZ1:
        pin = DIRZ1_PIN;
        if (du < 0)
        {
            duty = -du;
            dir = 0 + MTPOL_Z1;
        }
        else if (du >= 0)
        {
            duty = du;
            dir = 1 - MTPOL_Z1;
        }
        break;

        case nID_MAGTRQX2:
        pin = DIRX2_PIN;
        if (du < 0)
        {
            duty = -du;
            dir = 0 + MTPOL_X2;
        }
        else if (du >= 0)
        {
            duty = du;
            dir = 1 - MTPOL_X2;
        }
        break;

        case nID_MAGTRQY2:
        pin = DIRY2_PIN;
        if (du < 0)
        {
            duty = -du;
            dir = 0 + MTPOL_Y2;
        }
        else if (du >= 0)
        {
            duty = du;
            dir = 1 - MTPOL_Y2;
        }
        break;

        case nID_MAGTRQZ2:
        pin = DIRZ2_PIN;
        if (du < 0)
        {
            duty = -du;
            dir = 0 + MTPOL_Z2;
        }
        else if (du >= 0)
        {
            duty = du;
            dir = 1 - MTPOL_Z2;
        }
        break;

        default:
        duty = 0;
    }

    LOS_MagTrq_Set(pin, dir, duty);
}

void LOS_MagTrq_SetPol(uint8_t nID, uint8_t pol)
{
    switch(nID)
    {
        case nID_MAGTRQX1:
        if (pol == 0x55)
        {
            MTPOL_X1 = 0;
        }
        else if (pol == 0xAA)
        {
            MTPOL_X1 = 1;
        }
        break;

        case nID_MAGTRQY1:
        if (pol == 0x55)
        {
            MTPOL_Y1 = 0;
        }
        else if (pol == 0xAA)
        {
            MTPOL_Y1 = 1;
        }
        break;

        case nID_MAGTRQZ1:
        if (pol == 0x55)
        {
            MTPOL_Z1 = 0;
        }
        else if (pol == 0xAA)
        {
            MTPOL_Z1 = 1;
        }
        break;

        case nID_MAGTRQX2:
        if (pol == 0x55)
        {
            MTPOL_X2 = 0;
        }
        else if (pol == 0xAA)
        {
            MTPOL_X2 = 1;
        }
        break;

        case nID_MAGTRQY2:
        if (pol == 0x55)
        {
            MTPOL_Y2 = 0;
        }
        else if (pol == 0xAA)
        {
            MTPOL_Y2 = 1;
        }
        break;

        case nID_MAGTRQZ2:
        if (pol == 0x55)
        {
            MTPOL_Z2 = 0;
        }
        else if (pol == 0xAA)
        {
            MTPOL_Z2 = 1;
        }
        break;

        default:
    }
}


//动力学测试专用
void LOS_MagTrq_SetDyn(int8_t* trq)
{
    LOS_CAN_SendPack(LOS_CAN_NUM1,0xD1,trq, 6);
}

void LOS_MagTrq1_PWROn()
{
    gioSetBit(hetPORT2, MagTrq_SW1, 1);
}

void LOS_MagTrq1_PWROff()
{
    gioSetBit(hetPORT2, MagTrq_SW1, 0);
}


void LOS_MagTrq2_PWROn()
{
    gioSetBit(hetPORT2, MagTrq_SW2, 1);
}

void LOS_MagTrq2_PWROff()
{
    gioSetBit(hetPORT2, MagTrq_SW2, 0);
}
